#ifndef HELPER_H
#define HELPER_H

#include <stdio.h>

float helper_MatrixDet(float* m)
{
return  m[ 0]*m[ 5]*m[10]*m[15] + m[ 0]*m[ 9]*m[14]*m[ 7] + m[ 0]*m[13]*m[ 6]*m[11] +
    m[ 4]*m[ 1]*m[14]*m[11] + m[ 4]*m[ 9]*m[ 2]*m[15] + m[ 4]*m[13]*m[10]*m[ 3] +
    m[ 8]*m[ 1]*m[ 6]*m[15] + m[ 8]*m[ 5]*m[14]*m[ 3] + m[ 8]*m[13]*m[ 2]*m[ 7] +
    m[12]*m[ 1]*m[10]*m[ 7] + m[12]*m[ 5]*m[ 2]*m[11] + m[12]*m[ 9]*m[ 6]*m[ 3] -
    m[ 0]*m[ 5]*m[14]*m[11] - m[ 0]*m[ 9]*m[ 6]*m[15] - m[ 0]*m[13]*m[10]*m[ 7] -
    m[ 4]*m[ 1]*m[10]*m[15] - m[ 4]*m[ 9]*m[14]*m[ 3] - m[ 4]*m[13]*m[ 2]*m[11] -
    m[ 8]*m[ 1]*m[14]*m[ 7] - m[ 8]*m[ 5]*m[ 2]*m[15] - m[ 8]*m[13]*m[ 6]*m[ 3] -
    m[12]*m[ 1]*m[ 6]*m[11] - m[12]*m[ 5]*m[10]*m[ 3] - m[12]*m[ 9]*m[ 2]*m[ 7];
}

// m = in, mat = out
void helper_MatrixInv(float* mat, float* m)
{
	float determ = helper_MatrixDet(m);
	mat[0] = mat[5] = mat[10] = mat[15] = 0;

	if (determ == 0)
	{
	  printf("Matrix4, Inverse with determinant == 0");
	  return;
	}

	mat[ 0] = m[ 5]*m[10]*m[15] + m[ 9]*m[14]*m[ 7] + m[13]*m[ 6]*m[11] - m[ 5]*m[14]*m[11] - m[ 9]*m[ 6]*m[15] - m[13]*m[10]*m[ 7];
	mat[ 4] = m[ 4]*m[14]*m[11] + m[ 8]*m[ 6]*m[15] + m[12]*m[10]*m[ 7] - m[ 4]*m[10]*m[15] - m[ 8]*m[14]*m[ 7] - m[12]*m[ 6]*m[11];
	mat[ 8] = m[ 4]*m[ 9]*m[15] + m[ 8]*m[13]*m[ 7] + m[12]*m[ 5]*m[11] - m[ 4]*m[13]*m[11] - m[ 8]*m[ 5]*m[15] - m[12]*m[ 9]*m[ 7];
	mat[12] = m[ 4]*m[13]*m[10] + m[ 8]*m[ 5]*m[14] + m[12]*m[ 9]*m[ 6] - m[ 4]*m[ 9]*m[14] - m[ 8]*m[13]*m[ 6] - m[12]*m[ 5]*m[10];
	mat[ 1] = m[ 1]*m[14]*m[11] + m[ 9]*m[ 2]*m[15] + m[13]*m[10]*m[ 3] - m[ 1]*m[10]*m[15] - m[ 9]*m[14]*m[ 3] - m[13]*m[ 2]*m[11];
	mat[ 5] = m[ 0]*m[10]*m[15] + m[ 8]*m[14]*m[ 3] + m[12]*m[ 2]*m[11] - m[ 0]*m[14]*m[11] - m[ 8]*m[ 2]*m[15] - m[12]*m[10]*m[ 3];
	mat[ 9] = m[ 0]*m[13]*m[11] + m[ 8]*m[ 1]*m[15] + m[12]*m[ 9]*m[ 3] - m[ 0]*m[ 9]*m[15] - m[ 8]*m[13]*m[ 3] - m[12]*m[ 1]*m[11];
	mat[13] = m[ 0]*m[ 9]*m[14] + m[ 8]*m[13]*m[ 2] + m[12]*m[ 1]*m[10] - m[ 0]*m[13]*m[10] - m[ 8]*m[ 1]*m[14] - m[12]*m[ 9]*m[ 2];
	mat[ 2] = m[ 1]*m[ 6]*m[15] + m[ 5]*m[14]*m[ 3] + m[13]*m[ 2]*m[ 7] - m[ 1]*m[14]*m[ 7] - m[ 5]*m[ 2]*m[15] - m[13]*m[ 6]*m[ 3];
	mat[ 6] = m[ 0]*m[14]*m[ 7] + m[ 4]*m[ 2]*m[15] + m[12]*m[ 6]*m[ 3] - m[ 0]*m[ 6]*m[15] - m[ 4]*m[14]*m[ 3] - m[12]*m[ 2]*m[ 7];
	mat[10] = m[ 0]*m[ 5]*m[15] + m[ 4]*m[13]*m[ 3] + m[12]*m[ 1]*m[ 7] - m[ 0]*m[13]*m[ 7] - m[ 4]*m[ 1]*m[15] - m[12]*m[ 5]*m[ 3];
	mat[14] = m[ 0]*m[13]*m[ 6] + m[ 4]*m[ 1]*m[14] + m[12]*m[ 5]*m[ 2] - m[ 0]*m[ 5]*m[14] - m[ 4]*m[13]*m[ 2] - m[12]*m[ 1]*m[ 6];
	mat[ 3] = m[ 1]*m[10]*m[ 7] + m[ 5]*m[ 2]*m[11] + m[ 9]*m[ 6]*m[ 3] - m[ 1]*m[ 6]*m[11] - m[ 5]*m[10]*m[ 3] - m[ 9]*m[ 2]*m[ 7];
	mat[ 7] = m[ 0]*m[ 6]*m[11] + m[ 4]*m[10]*m[ 3] + m[ 8]*m[ 2]*m[ 7] - m[ 0]*m[10]*m[ 7] - m[ 4]*m[ 2]*m[11] - m[ 8]*m[ 6]*m[ 3];
	mat[11] = m[ 0]*m[ 9]*m[ 7] + m[ 4]*m[ 1]*m[11] + m[ 8]*m[ 5]*m[ 3] - m[ 0]*m[ 5]*m[11] - m[ 4]*m[ 9]*m[ 3] - m[ 8]*m[ 1]*m[ 7];
	mat[15] = m[ 0]*m[ 5]*m[10] + m[ 4]*m[ 9]*m[ 2] + m[ 8]*m[ 1]*m[ 6] - m[ 0]*m[ 9]*m[ 6] - m[ 4]*m[ 1]*m[10] - m[ 8]*m[ 5]*m[ 2];

	determ = 1.0f/determ;

	for (int num = 0; num < 16; ++num)
	  mat[num] *= determ;
}

#endif